#include <linux/sched.h>
#include <linux/sched/task.h>
#include <linux/thread_info.h>
#include <arm/v7m.h>
#include <asm/io.h>
#include <arm/bug.h>
#include <linux/printk.h>
#include <asm/ptrace.h>

struct pendsv_frame
{
    unsigned long exc_return;
    unsigned long s[16];
    struct exception_stack_frame e;
};

static void cur_do_resched(void)
{
    schedule();

    __raw_writel(V7M_SCB_ICSR_PENDSVSET, BASEADDR_V7M_SCB + V7M_SCB_ICSR);

    while (1)
        ;
}

void *__pendsv_do_work(void *sp)
{
    struct pendsv_frame *f = sp;
    struct thread_info *ti = current_thread_info();

    if (ti->esp == NULL)
    {
        struct pendsv_frame *f_new = f;

        f_new--;

        f_new->e.pc = cur_do_resched;
        f_new->e.lr = NULL;
        f_new->e.s[16] = 0;
        f_new->e.psr = 0x1000000;
        f_new->exc_return = 0xffffffe1 | EXC_RET_THREADMODE_PROCESSSTACK;

        ti->esp = (void *)sp;

        sp = (void *)f_new;
    }
    else
    {
        sp = ti->esp;
        ti->esp = NULL;
    }

    __raw_writel(V7M_SCB_ICSR_PENDSVCLR, BASEADDR_V7M_SCB + V7M_SCB_ICSR);

    return sp;
}
